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measuring an angular velocity of the sensor |^* 



integrating angular rate values 



normalizing the integrated angular rate values 
to produce an estimate of sensor orientation 



measuring a magnetic field vector 



measuring a gravity vector 

* — 



determining a measurement vector 
from magnetic field vector gravity vector 



calculating a computed measurement vector 
from estimated sensor orientation 
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generating an error vector 
that defines a criterion function 



minimizing the criterion function to 
output an orientation error estimate 



integrating the orientation error estimate 



normalizing the integrated error estimate 
to produce a new estimate of sensor orientation 



using the new estimate of sensor orientation to 
calculate computed measurement vector 5 1 3 and 
repeating steps 501-523 
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C START 1 



providing a starting estimate 
of sensor orientation quaternion 



measuring a magnetic field vector 



measuring a gravity vector 



determining a measurement vector 
from the magnetic field vector and the gravity vector 



calculating a computed measurement vector 
using quaternion mathematics 



generating an 6x1 error vector 
that defines a criterion function 
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minimizing the criterion function 
and outputting a 4x1 quaternion error estimate 



-[ 



integrating the quaternion error estimate 
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normalizing the integrated quaternion error estimate 
to produce a new estimated sensor orientation quaternion 



calculating a computed measurement vector 609 
using the new estimated sensor orientation quaternion and 
repeating steps 60j|-619 



